#include <GL/freeglut.h>
#include <GL/glut.h>

#include <ros/ros.h>
#include <ros/publisher.h>

#include <huniplacer_3d/huniplacer_3d.h>
#include <huniplacer_3d/Indicator.h>
#include <huniplacer_3d/ProgressBar.h>
#include <huniplacer_3d/LogPanel.h>

#include <huniplacer_3d/Callbacks.h>
#include <huniplacer_3d/DummyMotor.h>
#include <huniplacer_3d/IoHandlers.h>
#include <huniplacer_3d/Render.h>

#include <huniplacer_3d/demoMsg.h>
#include <huniplacer_3d/main.h>

#include <huniplacer/measures.h>

#include <huniplacer_3d/viewCheckSrv.h>


#include <deltarobotnode/motionSrv.h>

#include <stdio.h>
#include <string.h>

ros::ServiceClient motionClient;
ros::ServiceServer service1;

	
double testdouble = 4.0;
double motor_max_range;
	
huniplacerData data;

ros::Publisher pub;

huniplacerData* getHuniplacerData(){
#include <stdio.h>
#include <string.h>
	return &data;
}

/**
 * Initializes ROS to subscribe to several ROS topics
 * Also initializes the advertiser
 */
void initRos(int argc, char** argv){
	ros::init(argc, argv, "hunplacer_3d");
	data.rosdata.node = new ros::NodeHandle();

	data.rosdata.moveToSrv = data.rosdata.node->serviceClient<deltarobotnode::motionSrv>("moveTo");
	data.rosdata.motionReceiver = data.rosdata.node->subscribe("dummy", 1000, moveCallback);
	data.rosdata.crateReceiver = data.rosdata.node->subscribe("crateEvent", 1000, crateCallback);	
	data.rosdata.positionReceiver = data.rosdata.node->subscribe("pubDeltaPos", 100, positionCallback);
	data.rosdata.topviewReceiver = data.rosdata.node->subscribe("topview_info", 1000, topviewCallback);
	data.rosdata.demoProgressReceiver = data.rosdata.node->subscribe("demo_progress", 1000, demoProgressCallback);
	ROS_INFO("Huniplacer 3d viewer listening");
	
	ros::NodeHandle n;
	service1 = n.advertiseService("viewCheck", viewCheckCallback);
	pub = n.advertise<huniplacer_3d::demoMsg>("demo_info", 1000);
	
	motionClient = n.serviceClient<deltarobotnode::motionSrv>("moveTo");
	
   ros::Rate loop_rate(10);
}

/**
 * makes all parts of the GUI
 */
void initModel(int argc, char** argv){
	data.modeldata.ikmodel = new huniplacer::inverse_kinematics_impl(
			huniplacer::measures::BASE, huniplacer::measures::HIP,
			huniplacer::measures::EFFECTOR, huniplacer::measures::ANKLE,
			huniplacer::measures::HIP_ANKLE_ANGLE_MAX);

	data.modeldata.motor = new DummyMotor();

	data.modeldata.eb = effector_boundaries::generate_effector_boundaries(*data.modeldata.ikmodel, *data.modeldata.motor,
			3.0);
	data.modeldata.robot = new RobotModel(data.modeldata.ikmodel, data.modeldata.eb, data.modeldata.motor);

	data.paneldata.i_panel = new InfoPanel(0, 0);
	
	motionf motion;
	float gripper_x, gripper_y, gripper_z;
	data.modeldata.robot->getGripperPos(&gripper_x, &gripper_y, &gripper_z);
	point3 point3(gripper_x, gripper_y, gripper_z);

	data.modeldata.ikmodel->point_to_motion(point3, motion);

	data.paneldata.motor_angles[0] = new Indicator(100, 20, 75, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, motion.angles[0], measures::MOTOR_ROT_MIN, measures::MOTOR_ROT_MAX, const_cast<char *>(" radians"), const_cast<char *>("Motor 1"));
	data.paneldata.motor_angles[1] = new Indicator(300, 20, 75, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, motion.angles[1], measures::MOTOR_ROT_MIN, measures::MOTOR_ROT_MAX, const_cast<char *>(" radians"), const_cast<char *>("Motor 2"));
	data.paneldata.motor_angles[2] = new Indicator(500, 20, 75, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, motion.angles[2], measures::MOTOR_ROT_MIN, measures::MOTOR_ROT_MAX, const_cast<char *>(" radians"), const_cast<char *>("Motor 3"));
	data.paneldata.i_panel->add_part(data.paneldata.motor_angles[0]);
	data.paneldata.i_panel->add_part(data.paneldata.motor_angles[1]);
	data.paneldata.i_panel->add_part(data.paneldata.motor_angles[2]);
	
	data.paneldata.log = new LogPanel(900, 25, 400, 75, 1.0, 0.0, 0.0, 0.3, 0.3, 0.3, const_cast<char *>(""), const_cast<char *>("Log"));
	data.paneldata.i_panel->add_part(data.paneldata.log);

	//This panel part is not yet added to the i_panel. It mustn't be shown unless there is actually a demo running.
	data.paneldata.progressBar = new ProgressBar(700, 25, 150, 40, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0, 0, const_cast<char *>("%"), const_cast<char *>("Demo"));	
	
	data.modeldata.range.init(data.modeldata.eb);
	
	if (argc > 1) {
		data.modeldata.crate_xml = new char[strlen(argv[1])];
		strcpy(data.modeldata.crate_xml, argv[1]);
		std::cout << data.modeldata.crate_xml << std::endl;
	} else {
		data.modeldata.crate_xml = "Without the path to crates.xml not all features will work";
	}

	data.modeldata.draw_crates = true;
	
	data.modeldata.draw_motor_0 = true;
	data.modeldata.draw_motor_1 = true;
	data.modeldata.draw_motor_2 = true;
	
	data.modeldata.draw_glass_plate = true;
	
	data.modeldata.draw_beads = true;
	
	data.modeldata.draw_info_panel = true;
}


bool callMotionService(ros::ServiceClient& service, deltarobotnode::motionSrv motion) {
	service.call(motion);
	return motion.response.succeeded;
}

/**
 * Creates a window for the GUI and set handlers for all used functions
 *
 */
void initWindow(int argc, char** argv){
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
	//glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA | GLUT_MULTISAMPLE);
	glutInitDisplayString("rgba double depth=>12 samples>=8");
	glutInitWindowPosition(565, 0);
	glutInitWindowSize(500, 500);

	glutCreateWindow("HUniplacer 3D");

	// register callbacks
	glutMouseFunc(mouseHandler);
	glutSpecialFunc(specialHandler);
	glutSpecialUpFunc(specialUpHandler);
	glutKeyboardFunc(keyHandler);
	glutKeyboardUpFunc(keyUpHandler);
	glutDisplayFunc(render);
	glutReshapeFunc(resize);
	glutIdleFunc(render);
}

/**
 * This function is called to send a ROS message with a demo number to the crate_demo
 */
void sendDemo(int demo){
	 data.paneldata.i_panel->add_part(data.paneldata.progressBar);	
	
	 data.paneldata.progressBar->current_value = 0;	 
    huniplacer_3d::demoMsg msg;
    msg.name = "demo_message";
    msg.demo = demo;

    pub.publish(msg);
    ros::spinOnce();
}

/**
 * This function is called to send a ROS message with a destination and source crate and a destination and source position number
 */
void sendManualDemo(std::string crate_1, int box_1, std::string crate_2, int box_2) {
	std::cout << "send manual demo: " << crate_1 << ", box_1: " << box_1 << ", crate_2: " << crate_2 << ", box2: " << box_2 << std::endl;
	huniplacer_3d::demoMsg msg;
   msg.name = "demo_message";
   msg.demo = 1;
   msg.srcCrate = crate_1;
   msg.srcPos = box_1;
   msg.dstCrate = crate_2;
   msg.dstPos = box_2;

   pub.publish(msg);
   ros::spinOnce();
}

/**
 * This generation the context menu and binds it to RMB
 */
void setupMenus ()
{
    int toggleMenu, demoMenu, motorMenu;
    
    demoMenu = glutCreateMenu(showMenuItem);
    glutAddMenuEntry("manual mode", 001);
    glutAddMenuEntry("direct copy", 002);
    glutAddMenuEntry("copy with one spare hole", 003);
	 glutAddMenuEntry("complete mirrored copy", 004);
	 glutAddMenuEntry("horizontal mirror copy", 005);
	 glutAddMenuEntry("vertical mirror copy", 006);
	 glutAddMenuEntry("backward diagonal mirror copy", 007);
	 glutAddMenuEntry("forward diagonal mirror copy", 8);
	 glutAddMenuEntry("flexible demo", 9);
	 glutAddMenuEntry("color demo", 10);
    
    motorMenu = glutCreateMenu(showMenuItem);
    glutAddMenuEntry("motor 0", 111);
    glutAddMenuEntry("motor 1", 112);
	 glutAddMenuEntry("motor 2", 113);
    
    toggleMenu = glutCreateMenu(showMenuItem);
    glutAddSubMenu("show/hide motors", motorMenu);
	 glutAddMenuEntry("Toggle glass_plate", 120);
    glutAddMenuEntry("Toggle live crates", 130);
	 //glutAddMenuEntry("Toggle live beats", 140);   
    
    glutCreateMenu(showMenuItem);
    glutAddSubMenu("demo's", demoMenu);
    glutAddSubMenu("toggle objects", toggleMenu);
    
    glutAttachMenu(GLUT_RIGHT_BUTTON);
}

int main(int argc, char** argv) {
	initRos(argc, argv);
	initModel(argc, argv);
	initWindow(argc, argv);
	setupMenus();
	
	glutMainLoop();

	return 0;
}
